package image;

import java.awt.AWTEvent;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.MouseInfo;
import java.awt.Point;
import java.awt.Toolkit;
import java.awt.event.AWTEventListener;
import java.awt.event.KeyEvent;
import java.awt.event.MouseAdapter;
import java.awt.event.MouseEvent;
import java.awt.event.WindowAdapter;
import java.awt.event.WindowEvent;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.awt.Frame;
import javax.media.opengl.GL;
import javax.media.opengl.GL2;
import javax.media.opengl.GLAutoDrawable;
import javax.media.opengl.GLEventListener;
import javax.media.opengl.awt.GLCanvas;
import com.github.sarxos.webcam.Webcam;
/**The main class for the image processing part*/
public class ImageMain implements GLEventListener
{	
	static int[] minValue = new int[4], minSaturation = new int[4];
	static String[] componentNames = {"Red", "Green", "Blue", "White"};
	static Color[] color = {Color.red, Color.green, Color.blue, Color.white};
	static int RED_INDEX = 0;
	static int GREEN_INDEX = 1;
	static int BLUE_INDEX = 2;
	static int WHITE_INDEX = 3;
	
	static final int blockSize = 3;
	
	Grid grid;
	
	static Frame frame;
	static GLCanvas canvas;
	static boolean lockCorners = false;
	static boolean maskToggle = false;
	double smoothedSize = 0;
	public static void main(String[] args) throws IOException
	{
		Settings.readSettings();
		new CalibrationFrame();
		
		frame = new Frame();
		canvas = new GLCanvas();

		new ImageMain(canvas);

		frame.add(canvas);
		frame.addWindowListener(new WindowAdapter(){
			public void windowClosing(WindowEvent we){
				System.out.println("Clean exit");
				Settings.writeSettings();
				System.exit(0);
			}
		});

		frame.pack();
		frame.setVisible(true);
		frame.setLocationRelativeTo(null);

		Thread t = new Thread()
		{
			@Override public void run()
			{
				while(true)
				{
					canvas.repaint();
				}
			}
		};
		t.setDaemon(true);
		t.start();
	}

	public static volatile int webcamDelay = 0;
	public static ByteBuffer webcamBuffer = ByteBuffer.allocate(0);
	public static WebcamFetcher fetcher = null;
	public static Dimension d = null, td = null;
	public static Webcam webcam = null;

	GL2 gl;
	int inputTexture, readbackTexture;
	ByteBuffer textureBuffer, readbackBuffer;

	V3F[] mediansForCorners;
	Server server;
	/**Thread to retrieve data from the webcam object with minimal blocking of the program.*/
	class WebcamFetcher extends Thread
	{
		@Override
		public void run()
		{
			long oldTime = System.currentTimeMillis();
			ByteBuffer b = webcam.getImageBytes();
			webcamDelay = (int)(System.currentTimeMillis()-oldTime);
			synchronized(webcamBuffer) 
			{
				webcamBuffer = b;
			}
		}
	}

	public ImageMain(GLCanvas p)
	{
		p.addMouseListener(new MouseAdapter()
		{
			@Override
			public void mousePressed(MouseEvent e)
			{

			}
		});
		Toolkit.getDefaultToolkit().addAWTEventListener(new AWTEventListener() {
				@Override
				public void eventDispatched(AWTEvent e) {
					if(e instanceof KeyEvent && ((KeyEvent)e).getID() == KeyEvent.KEY_PRESSED) {
						int c = ((KeyEvent)e).getKeyCode();
						if(c == KeyEvent.VK_L)
							lockCorners = !lockCorners;
						if(c == KeyEvent.VK_M)
							maskToggle = !maskToggle;
						if(c == KeyEvent.VK_C)
							grid.clear();
					}
				}
			}, AWTEvent.KEY_EVENT_MASK);
		
		System.out.println(Webcam.getWebcams());
		webcam = Webcam.getWebcams().get(0);
		Dimension[] sizes = webcam.getViewSizes();
		System.out.println(Arrays.toString(sizes));
		d = sizes[sizes.length-1];
		td = new Dimension(d.width, (int) ((d.width*4f)/7.7f));
		textureBuffer = ByteBuffer.allocate(d.width*d.height*3);
		readbackBuffer = ByteBuffer.allocate(d.width*d.height*3);
		webcam.setViewSize(d);
		webcam.setImageTransformer(null);
		webcam.open();
		Dimension cSize = new Dimension(webcam.getViewSize());
		
		grid = new Grid(new V3F(), blockSize, blockSize,
					(int)Math.ceil((double)d.width/blockSize), 
					(int)Math.ceil((double)d.height/blockSize));

		cSize.width*=2;
		p.setPreferredSize(cSize);

		p.addGLEventListener(this);
		fetcher = new WebcamFetcher();
		fetcher.start();

		server = new Server();
	}

	@Override
	public void display(GLAutoDrawable e)
	{
		long oldTime = System.currentTimeMillis();
		gl = (GL2)e.getGL();

		gl.glClearColor(0, 0, 0, 0);
		gl.glClear(GL2.GL_COLOR_BUFFER_BIT);

		gl.glViewport(0, 0, e.getWidth()/2, e.getHeight());

		gl.glMatrixMode(GL2.GL_MODELVIEW);
		gl.glLoadIdentity();
		gl.glOrthof(0, d.width, d.height, 0, -1, 1);
		gl.glDisable(GL2.GL_CULL_FACE);

		try{fetcher.join();}catch (InterruptedException e1){e1.printStackTrace();}

		synchronized(webcamBuffer) 
		{
			fetcher = new WebcamFetcher();
			textureBuffer.put(webcamBuffer);
			textureBuffer.rewind();
			fetcher.start();
		}
		byte[] data = textureBuffer.array();

		if(!lockCorners)
		{
			mediansForCorners = Manip.findCorners(data, d);
		}

		gl.glBindTexture(GL2.GL_TEXTURE_2D, inputTexture);
		gl.glTexSubImage2D(GL2.GL_TEXTURE_2D, 0, 0, 0, d.width, d.height, GL2.GL_RGB, GL2.GL_UNSIGNED_BYTE, textureBuffer);
		textureBuffer.rewind();
		gl.glColor3f(1,1,1);
		gl.glEnable(GL2.GL_TEXTURE_2D);
		DrawUtil.renderUndeformed(gl, mediansForCorners, d, td);
		gl.glDisable(GL2.GL_TEXTURE_2D);


		gl.glReadBuffer(GL2.GL_BACK);
		gl.glBindTexture(GL2.GL_TEXTURE_2D, readbackTexture);
		gl.glReadPixels(0, 0, d.width, d.height, GL2.GL_RGB, GL2.GL_UNSIGNED_BYTE, readbackBuffer);
		
		
		data = readbackBuffer.array();
		
		ArrayList<Integer> startX = new ArrayList<Integer>();
		ArrayList<Integer> startY = new ArrayList<Integer>();
		ArrayList<Integer> blueX = new ArrayList<Integer>();
		ArrayList<Integer> blueY = new ArrayList<Integer>();
		
		for(int y = 0; y<d.height; y+=1) {
			for(int x = 0; x<d.width; x+=1) {
				
				int i = 3*(x+(d.height-1-y)*d.width);
				int r = data[i]&0xFF;
				int g = data[i+1]&0xFF;
				int b = data[i+2]&0xFF;
				
				float[] hsv = this.rgb2hsv(r/255f, g/255f, b/255f, null);

				byte rVal = (byte)(((r > minValue[RED_INDEX] && hsv[1] > minSaturation[RED_INDEX]/255f) ||
						(hsv[2] > minValue[WHITE_INDEX]/255f && hsv[1] < minSaturation[WHITE_INDEX]/255f))?255:0);
				byte gVal = (byte)(((g > minValue[GREEN_INDEX] && hsv[1] > minSaturation[GREEN_INDEX]/255f) ||
						(hsv[2] > minValue[WHITE_INDEX]/255f && hsv[1] < minSaturation[WHITE_INDEX]/255f))?255:0);
				byte bVal = (byte)(((b > minValue[BLUE_INDEX] && hsv[1] > minSaturation[BLUE_INDEX]/255f) ||
						(hsv[2] > minValue[WHITE_INDEX]/255f && hsv[1] < minSaturation[WHITE_INDEX]/255f))?255:0);

				data[i] = rVal;
				data[i+1] = gVal;
				data[i+2] = bVal;
				
				if(rVal != 0 && gVal == 0 && bVal == 0) {
					startX.add(x);
					startY.add(y);
				}
				if(rVal == 0 && gVal == 0 && bVal != 0) {
					blueX.add(x);
					blueY.add(y);
				}
			}
		}
		
		Collections.sort(startX);
		Collections.sort(startY);
		Collections.sort(blueX);
		Collections.sort(blueY);

		float ax = -1, ay = -1, bx = -1, by = -1;
		if(startX.size()>0) {
			ax = startX.get(startX.size()/2);
			ay = startY.get(startY.size()/2);
		}

		if(blueX.size()>0) {
			bx = blueX.get(blueX.size()/2);
			by = blueY.get(blueY.size()/2);
		}

		double size = Calc.dist(ax, ay, bx, by);
		V3F position = new V3F(ax+bx*2.8f, ay+by*2.8f, 0).multiply(1f/(1+2.8f));
		//double px = (ax+bx)/2.0;
		//double py = (ay+by)/2.0;
		
		

		double r = size;
		smoothedSize = (r+smoothedSize*3)/4;
		
		Manip.setOpenBlocked(grid, data, d, ax, ay, position.x(), position.y(), bx, by, smoothedSize);
		
		Point mouseLoc = MouseInfo.getPointerInfo().getLocation();
		Point frameLoc = canvas.getLocationOnScreen();
		V3F goal = new V3F((mouseLoc.x-frameLoc.x), (mouseLoc.y-frameLoc.y), 0);
		V3F[] path = grid.findPath(position, goal, false, 16, 11 );
		
		//if(path == null)
		//	System.err.println("No path");
		
		double currentDir = Calc.dir(ax, ay, bx, by);

		double targetDir;
		double targetDx;
		double targetDy;
		if(path ==  null) {
			targetDir = -1;
			targetDx = -2;
			targetDy = -2;
		} else if(path.length < 4) {
			targetDir = -1;
			targetDx = -1;
			targetDy = -1;
		}
		else {
			int maxInd = -1;
			//First, try to find the largest distance we can move without turning
			
			if(maxInd == -1){
			//Find the largest distance we can without generating a large cross-track error
			maxInd = Math.min(path.length-1, 40	);
			double maxError = Integer.MAX_VALUE;
				while(maxInd > 4) {
					maxError = 0;
					V3F dir = path[maxInd].clone().sub(path[0]).normalize();
					for(int i = 1; i<maxInd; i++) {
						V3F point = path[i];
						V3F base = path[0];
						V3F offset = point.clone().sub(base);
						V3F ref = base.clone().add(dir, offset.dot(dir));
						double error = ref.distance(point);
						if(error > maxError)
							maxError = error;
					}
					int threshold = 8-(int)(6*maxInd/40f);
					
					if(maxError > threshold)
						maxInd -= 4;
					else {
						break;
					}
				}
			}
			//If we could move a short distance without turning instead, then do that
			if(Math.abs(Calc.dirDiff(
						currentDir, 
						Calc.dir(path[0].x(), path[0].y(), path[maxInd].x(), path[maxInd].y())
					)) > 12*Calc.toRad) {
				int largestNonTurningDistance = -1;
				for(int shortCandidate = 1; shortCandidate < maxInd; shortCandidate++) {
					if(Math.abs(Calc.dirDiff(
							currentDir, 
							Calc.dir(path[0].x(), path[0].y(), path[shortCandidate].x(), path[shortCandidate].y())
						)) < 12*Calc.toRad){
						largestNonTurningDistance = shortCandidate;
					}
				}
				if(largestNonTurningDistance != -1)
					maxInd = largestNonTurningDistance;
			}
				 
			
			targetDir = Calc.dir(path[0].x(), path[0].y(), path[maxInd].x(), path[maxInd].y());
			targetDx = ((path[0].x()-path[maxInd].x())/td.width);
			targetDy = ((path[0].y()-path[maxInd].y())/td.height);
		}
		double diff = Calc.dirDiff(currentDir, targetDir);
		//System.out.println((Calc.toDeg*diff));
		
		
		
		double dir = Calc.dir(ax, ay, bx, by);
		float h = (float)Math.cos(dir)*12;
		float v = (float)-Math.sin(dir)*12;

		gl.glBegin(GL2.GL_TRIANGLE_STRIP);
		gl.glColor4f(1, 1, 0, 0.85f);
		gl.glVertex2f(ax+v-h, ay-h-v);
		gl.glVertex2f(ax-v-h, ay+h-v);
		gl.glColor4f(0, 1, 1, 0.85f);
		gl.glVertex2f(bx+v+h, by-h+v);
		gl.glVertex2f(bx-v+h, by+h+v);
		gl.glEnd();

		gl.glColor3f(1, 1, 1);

		if(path != null) 
		{
			gl.glBegin(GL2.GL_LINE_STRIP);
			gl.glColor3f(0, 0, 1);
			for(int i = 0; i<path.length; i++) 
			{
				gl.glVertex2f(path[i].x(), path[i].y());
			}
			gl.glEnd();
		}

		
		gl.glViewport(e.getWidth()/2, 0, e.getWidth()/2, e.getHeight());
		
		gl.glEnable(GL2.GL_TEXTURE_2D);
		gl.glBindTexture(GL2.GL_TEXTURE_2D, inputTexture);
		
		//Minimum and maximum texture coordinate. The mask image is flipped.
		final float itcy, atcy;
		
		if(maskToggle) {
			gl.glTexSubImage2D(GL2.GL_TEXTURE_2D, 0, 0, 0, d.width, d.height, GL2.GL_RGB, GL2.GL_UNSIGNED_BYTE, readbackBuffer);
			readbackBuffer.rewind();
			itcy = 1;
			atcy = 0;
		}
		else {
			itcy = 0;
			atcy = 1;
		}

		gl.glColor3f(1,1,1);
		gl.glBegin(GL2.GL_TRIANGLE_STRIP);
		gl.glTexCoord2f(0, itcy);
		gl.glVertex2f(0,0);
		gl.glTexCoord2f(0, atcy);
		gl.glVertex2f(0,d.height);
		gl.glTexCoord2f(1, itcy);
		gl.glVertex2f(d.width,0);
		gl.glTexCoord2f(1, atcy);
		gl.glVertex2f(d.width,d.height);
		gl.glEnd();


		gl.glDisable(GL2.GL_TEXTURE_2D);
		gl.glBindTexture(GL2.GL_TEXTURE_2D, 0);
		
		if(maskToggle) {
			grid.render(gl);
		} else {
			//Corners
			gl.glColor4f(1, 0, 1, 0.75f);
			gl.glBegin(GL2.GL_TRIANGLES);
			for(int i = 0; i<4; i++) {
				gl.glVertex2f(mediansForCorners[i].x()-16, mediansForCorners[i].y()-12);
				gl.glVertex2f(mediansForCorners[i].x()+16, mediansForCorners[i].y()-12);
				gl.glVertex2f(mediansForCorners[i].x(), mediansForCorners[i].y()+20);
			}
			gl.glEnd();
		}
		
		server.sendFloat((float)(Calc.toDeg*diff));
		server.sendFloat((float)targetDx);
		server.sendFloat((float)targetDy);

		int imageProcessingDelay = (int)(System.currentTimeMillis()-oldTime);
		//System.out.println("Delay: "+imageProcessingDelay+", "+webcamDelay);
		server.sendInt(imageProcessingDelay + webcamDelay);

		try {
			Thread.sleep(20);
		}
		catch (InterruptedException e1) {
			e1.printStackTrace();
		}
	}


	@Override
	public void dispose(GLAutoDrawable e)
	{}

	@Override
	public void init(GLAutoDrawable e)
	{
		gl = (GL2)e.getGL();
		int[] ibuf = new int[2];
		gl.glGenTextures(2, ibuf, 0);
		inputTexture = ibuf[0];
		readbackTexture = ibuf[1];

		gl.glEnable(GL2.GL_BLEND);
		gl.glBlendFunc(GL.GL_SRC_ALPHA, GL.GL_ONE_MINUS_SRC_ALPHA);

		gl.glBindTexture(GL2.GL_TEXTURE_2D, inputTexture);
		gl.glTexParameteri(GL2.GL_TEXTURE_2D, GL2.GL_TEXTURE_WRAP_S, GL2.GL_CLAMP_TO_BORDER);
		gl.glTexParameteri(GL2.GL_TEXTURE_2D, GL2.GL_TEXTURE_WRAP_T, GL2.GL_CLAMP_TO_BORDER);
		gl.glTexParameteri(GL2.GL_TEXTURE_2D, GL2.GL_TEXTURE_MIN_FILTER, GL2.GL_LINEAR);
		gl.glTexParameteri(GL2.GL_TEXTURE_2D, GL2.GL_TEXTURE_MAG_FILTER, GL2.GL_NEAREST);
		gl.glTexImage2D(GL2.GL_TEXTURE_2D, 0, GL2.GL_RGB, d.width, d.height, 
				0, GL2.GL_RGB, GL2.GL_UNSIGNED_BYTE, textureBuffer);
		textureBuffer.rewind();

		gl.glBindTexture(GL2.GL_TEXTURE_2D, readbackTexture);
		gl.glTexParameteri(GL2.GL_TEXTURE_2D, GL2.GL_TEXTURE_WRAP_S, GL2.GL_CLAMP_TO_BORDER);
		gl.glTexParameteri(GL2.GL_TEXTURE_2D, GL2.GL_TEXTURE_WRAP_T, GL2.GL_CLAMP_TO_BORDER);
		gl.glTexParameteri(GL2.GL_TEXTURE_2D, GL2.GL_TEXTURE_MIN_FILTER, GL2.GL_LINEAR);
		gl.glTexParameteri(GL2.GL_TEXTURE_2D, GL2.GL_TEXTURE_MAG_FILTER, GL2.GL_NEAREST);
		gl.glTexImage2D(GL2.GL_TEXTURE_2D, 0, GL2.GL_RGB, d.width, d.height, 
				0, GL2.GL_RGB, GL2.GL_UNSIGNED_BYTE, null);

		gl.glBindTexture(GL2.GL_TEXTURE_2D, 0);
	}

	@Override
	public void reshape(GLAutoDrawable e, int x, int y, int w, int h)
	{}


	float[] rgb2hsv(float r, float g, float b, float[] out)
	{
		if(out == null)
			out = new float[3];

		float      min, max, delta;

		min = r < g ? r : g;
		min = min  < b ? min  : b;

		max = r > g ? r : g;
		max = max  > b ? max  : b;

		out[2] = max;                                // v
		delta = max - min;
		if( max > 0.0 ) {
			out[1] = (delta / max);                  // s
		} else {
			// r = g = b = 0                        // s = 0, v is undefined
			out[1] = 0.0f;
			out[0] = 0.0f;                            // its now undefined
			return out;
		}
		if( r >= max )                           // > is bogus, just keeps compilor happy
			out[0] = ( g - b ) / delta;        // between yellow & magenta
		else
			if( g >= max )
				out[0] = 2.0f + ( b - r ) / delta;  // between cyan & yellow
			else
				out[0] = 4.0f + ( r - g ) / delta;  // between magenta & cyan

		out[0] *= 60.0/360.0;                              // normalised

		if( out[0] < 0.0 )
			out[0] += 1.0;

		return out;
	}


	float[] hsv2rgb(float h, float s, float v, float[] out)
	{
		if(out == null)
			out = new float[3];
		float      hh, p, q, t, ff;
		int        i;

		if(s <= 0.0) {       // < is bogus, just shuts up warnings
			out[0] = v;
			out[1] = v;
			out[2] = v;
			return out;
		}
		hh = h;
		if(hh >= 360.0) hh = 0.0f;
		hh /= 60.0/360.0;
		i = (int)hh;
		ff = hh - i;
		p = v * (1.0f - s);
		q = v * (1.0f - (s * ff));
		t = v * (1.0f - (s * (1.0f - ff)));

		switch(i) {
		case 0:
			out[0] = v;
			out[1] = t;
			out[2] = p;
			break;
		case 1:
			out[0] = q;
			out[1] = v;
			out[2] = p;
			break;
		case 2:
			out[0] = p;
			out[1] = v;
			out[2] = t;
			break;

		case 3:
			out[0] = p;
			out[1] = q;
			out[2] = v;
			break;
		case 4:
			out[0] = t;
			out[1] = p;
			out[2] = v;
			break;
		case 5:
		default:
			out[0] = v;
			out[1] = p;
			out[2] = q;
			break;
		}
		return out;     
	}
}
